#ifndef __ROB_KINEMATICS_H__
#define __ROB_KINEMATICS_H__

#include "rob_common.h"

/* Kinematics Configuration Structure */
typedef struct {
    float32 L0_f32;  /* Base height (mm) */
    float32 L1_f32;  /* First link length (mm) */
    float32 L2_f32;  /* Second link length (mm) */
    float32 L3_f32;  /* End effector length (mm) */
} Rob_KinConfig_Type;

/* Joint Angles Structure */
typedef struct {
    float32 Theta0_f32;  /* Base rotation angle (degrees) */
    float32 Theta1_f32;  /* Shoulder angle (degrees) */
    float32 Theta2_f32;  /* Elbow angle (degrees) */
    float32 Theta3_f32;  /* Wrist pitch angle (degrees) */
    float32 Theta4_f32;  /* Wrist roll angle (degrees) */
    float32 Theta5_f32;  /* Gripper angle (degrees) */
} Rob_JointAngles_Type;

/* Cartesian Position Structure */
typedef struct {
    float32 X_f32;      /* X position (mm) */
    float32 Y_f32;      /* Y position (mm) */
    float32 Z_f32;      /* Z position (mm) */
    float32 Alpha_f32;  /* End effector pitch angle (degrees) */
    float32 Beta_f32;   /* End effector roll angle (degrees) */
} Rob_CartesianPos_Type;

/* Kinematics State Structure */
typedef struct {
    Rob_KinConfig_Type Config;
    Rob_JointAngles_Type CurrentAngles;
    Rob_JointAngles_Type TargetAngles;
    Rob_CartesianPos_Type CurrentPos;
    Rob_CartesianPos_Type TargetPos;
    Rob_Status_Type Status;
} Rob_KinState_Type;

/* Joint Limits Structure */
typedef struct {
    float32 Min_f32;  /* Minimum angle (degrees) */
    float32 Max_f32;  /* Maximum angle (degrees) */
} Rob_JointLimit_Type;

typedef struct {
    Rob_JointLimit_Type Limits[ROB_SERVO_NUM];
} Rob_JointLimits_Type;

/* Function Declarations */

/* Initialization Functions */
Rob_Status_Type Rob_KinInit(Rob_KinState_Type *State_pst, 
                           const Rob_KinConfig_Type *Config_cpst);

/* Forward Kinematics */
Rob_Status_Type Rob_KinForward(const Rob_JointAngles_Type *Angles_cpst,
                              const Rob_KinConfig_Type *Config_cpst,
                              Rob_CartesianPos_Type *Position_pst);

/* Inverse Kinematics */
Rob_Status_Type Rob_KinInverse(const Rob_CartesianPos_Type *Position_cpst,
                              const Rob_KinConfig_Type *Config_cpst,
                              Rob_JointAngles_Type *Angles_pst);

/* Jacobian Matrix Calculation */
Rob_Status_Type Rob_KinJacobian(const Rob_JointAngles_Type *Angles_cpst,
                               const Rob_KinConfig_Type *Config_cpst,
                               float32 Jacobian_pf32[6][6]);

/* Joint Limit Checking */
Rob_Status_Type Rob_KinCheckLimits(const Rob_JointAngles_Type *Angles_cpst,
                                  const Rob_JointLimits_Type *Limits_cpst);

/* Angle Conversion Functions */
float32 Rob_KinDegToRad_f32(float32 Degrees_f32);
float32 Rob_KinRadToDeg_f32(float32 Radians_f32);

/* Configuration Functions */
void Rob_KinSetConfig(Rob_KinState_Type *State_pst,
                     float32 L0_f32, float32 L1_f32,
                     float32 L2_f32, float32 L3_f32);

void Rob_KinSetLimits(Rob_JointLimits_Type *Limits_pst,
                     uint8 Joint_u8, float32 Min_f32, float32 Max_f32);

/* State Management */
Rob_Status_Type Rob_KinSetTargetPos(Rob_KinState_Type *State_pst,
                                   float32 X_f32, float32 Y_f32, float32 Z_f32,
                                   float32 Alpha_f32, float32 Beta_f32);

Rob_Status_Type Rob_KinUpdateState(Rob_KinState_Type *State_pst);

#endif /* __ROB_KINEMATICS_H__ */
